clear all;clc;
format shortEng
p = 1140;
g = 9.8;
pi = 3.1415926;
diameter = 0.15;%网目直径，单位m
E = 8.3e9;%弹性模量Pa
G = 3.2e9;%剪切模量Pa
A = pi*diameter^2/4;%实心圆截面
I = pi*diameter^4/64;%惯性矩,单位m^4
Ip = 2*I;%极惯性矩，单位m^4
It= pi*diameter^3/16;%抗扭惯性矩，单位m^3
L0 = 1;
Ir = A*L0*p/9.8*diameter^2/4;%转动惯量,单位kg*m^2;
scalefactor = 0.2;
hight = 11;
width = 11;
numNode = hight*width; %节点个数
elecont = 1;
numDOF = numNode*6; %自由度
displacement = zeros(numDOF,1);
force = zeros(numDOF,1);
F = zeros(numDOF,1);
force_static = zeros(numDOF,1);
force_internal = zeros(numDOF,1);
stiffness = zeros(numDOF);
total_M = zeros(numDOF);
nonlinear_stiffness = zeros(numDOF);
numEle = (hight-1)*width + (width-1)*hight;

%环境参数设置
V = 1;%水流速度m/s
deg = 0;%水流角度deg
Vx = V * cos(pi*deg/180);%x方向水流速度m/s
Vy = V * sin(pi*deg/180);%y方向水流速度m/s
Ca = 1;%附加质量系数
pw = 1.025*1000;%海水密度kg/m^3
Cd = 1.0;%阻尼系数
damp_param_2 = 0.1;%Rayleigh结构阻尼系数（只考虑和刚度矩阵相关）

%波浪设置
Input.WaterDepth = 50;
Input.T = 10;
Input.Amplitude = 0;
Input.LocalDepth = 0;
Input.PhaseAng = 0;%PhaseAng初始偏移设为0就好
Input.Time = 0;%时刻

Input.Dis_X = 0;
Input.Dis_Y = 0;
Input.WaveDir = 0;

%动态分析参数信息
t_gap = 0.32;
c0 = 1/(1/4*t_gap^2); c1 = (1/2)/(1/4*t_gap); c2 = 1/(1/4*t_gap); c3 = 1/(2*1/4) - 1;
c4 = (1/2)/(1/4) - 1; c5 = t_gap/2*((1/2)/(1/4)-2); c6 = t_gap*(1-1/2); c7 = 1/2*t_gap;

%输入点边信息
for i = 1:width
    for j = 1:hight
        nodeCoord((i - 1)*hight + j,:) = [0 (i-1)*L0 -(j-1)*L0-10];
        force_static(((i - 1)*hight + j)*6-5) = 0;
        if i>1
            EleNode(elecont,:) = [(i - 2)*hight+j (i - 1)*hight+j];
            elecont=elecont+1;
        end
    end
    for j = 1:hight-1
        EleNode(elecont,:) = [(i - 1)*hight+j (i - 1)*hight+j+1];
        elecont=elecont+1;
    end
end

% %填加约束，边固定
for i = 1:hight*2+width*2-4
    if i<=hight
        restrainedDof(i*6-5:i*6-3) = [i*6-5 i*6-4 i*6-3];
    end
    if hight<i && i<=2*hight
        temp1 = numNode-2*hight+i;
        restrainedDof(i*6-5:i*6-3) = [temp1*6-5 temp1*6-4 temp1*6-3];
    end
    if 2*hight<i && i<=2*hight+width-2
         temp2 = (i-2*hight)*hight;
         restrainedDof(i*6-5:i*6-3) = [(temp2+1)*6-5 (temp2+1)*6-4 (temp2+1)*6-3];
    end
    if 2*hight+width-2<i && i<=hight*2+width*2-4
         temp3 = (i-2*hight-width+3)*hight;
         restrainedDof(i*6-5:i*6-3) = [temp3*6-5 temp3*6-4 temp3*6-3];
    end
end

% restrainedDof(1:6) = [1 2 3 4 5 6];
% restrainedDof(7:12) = [numNode*6-5 numNode*6-4 numNode*6-3 numNode*6-2 numNode*6-1 numNode*6];


%考虑重力浮力
for i = 1:numEle
    noindex = EleNode(i,:);
    force_static(noindex(1)*6 - 3) =force_static(noindex(1)*6 - 3) - 1/2*(p*g*A*L0 - 1.025*10^3*9.8*A*L0);
    force_static(noindex(2)*6 - 3) =force_static(noindex(2)*6 - 3) - 1/2*(p*g*A*L0 - 1.025*10^3*9.8*A*L0);
end

%矩阵与迭代设置
Q_effective = zeros(numDOF,1);
activeDof = setdiff([1:numDOF]',restrainedDof);%去除节点自由度的约限
node_displacement = zeros(numDOF,1);
node_displacement_new = zeros(numDOF,1);
displacement_gap = zeros(numDOF,1);
node_speed = zeros(numDOF,1);
node_speed_new = zeros(numDOF,1);
node_accelerated_speed = zeros(numDOF,1);
node_accelerated_speed_new = zeros(numDOF,1);
damp_matrix = zeros(numDOF);

displacement_gap(:,1)= 0;
nodeCoord_new(:,1) = nodeCoord(:,1);
nodeCoord_new(:,2) = nodeCoord(:,2);
nodeCoord_new(:,3) = nodeCoord(:,3);
force_internal = zeros(numDOF,1);
for i = 1:200
    %     while (any(abs(node_displacement_new - temp_displacement) > 0.000005))
    %         K_tangential(activeDof,activeDof) = node_displacement_new(activeDof)/Q_effective(activeDof);
    %         temp_displacement = node_displacement_new;
    %         node_displacement_new(activeDof) = K_tangential(activeDof,activeDof)\Q_effective(activeDof);
    %     end
    
    %每个时间步计算切线刚度矩阵

    label_1 = 1;
    count_temp = 0;
    while label_1
        xx = nodeCoord_new(:,1);
        yy = nodeCoord_new(:,2);
        zz = nodeCoord_new(:,3);
        force = force_static;
        stiffness = zeros(numDOF);
        total_M = zeros(numDOF);
        nonlinear_stiffness = zeros(numDOF);
        
        for j = 1:numEle
            noindex = EleNode(j,:);
            deltax = xx(noindex(2))-xx(noindex(1));
            deltay = yy(noindex(2))-yy(noindex(1));
            deltaz = zz(noindex(2))-zz(noindex(1));
            L = sqrt(deltax*deltax + deltay*deltay + deltaz*deltaz);
            axial_force(j) = (L-L0)/L0*E*A;
            %坐标转换矩阵
            lx = deltax/L;
            mx = deltay/L;
            nx = deltaz/L;
            D = (lx^2+mx^2)^(1/2);
            if D == 0
                ly =0;
                my = 1;
                ny = 0;
                lz = -1;
                mz = 0;     
                nz = 0;
            else
                ly = -mx/D;
                my = lx/D;
                ny = 0;
                lz = -lx*nx/D;
                mz = -mx*nx/D;
                nz = D;
            end
            T = coordinate_transform_matrix(lx,mx,nx,ly,my,ny,lz,mz,nz);
            
            Tf = coordinate_transform_matrix_force(lx,mx,nx,ly,my,ny,lz,mz,nz);
            %刚度矩阵
            EAL = E*A/L;
            EIL1 = E*I/L;
            EIL2 = 6*E*I/(L*L);
            EIL3 = 12*E*I/(L*L*L);
            GIL = G*Ip/L;
            eleK = stiffness_matrix(EAL,EIL1,EIL2,EIL3,GIL);
            
%             if i == 1
%                 eleK_max = 0
%                 for kk = 1,12
%                    if eleK_max < eleK(kk,kk)
%                        eleK_max = eleK(kk,kk)
%                    end
%                 end
% 
%                 for kk = 1,12
%                     eleK(kk,kk) = eleK(kk,kk) + eleK_max*(1/count_temp)^2;
%                 end
%             end
            
            %几何非线性刚度矩阵
            u1 = displacement_gap(noindex(1)*6-5);
            v1 = displacement_gap(noindex(1)*6-4);
            w1 = displacement_gap(noindex(1)*6-3);
            u2 = displacement_gap(noindex(2)*6-5);
            v2 = displacement_gap(noindex(2)*6-4);
            w2 = displacement_gap(noindex(2)*6-3);
            cx1 = displacement_gap(noindex(1)*6-2);
            cy1 = displacement_gap(noindex(1)*6-1);
            cz1 = displacement_gap(noindex(1)*6);
            cx2 = displacement_gap(noindex(2)*6-2);
            cy2 = displacement_gap(noindex(2)*6-1);
            cz2 = displacement_gap(noindex(2)*6);
            param1 = inv(Tf')*[u1;v1;w1];
            param2 = inv(Tf')*[u2;v2;w2];
            param3 = inv(Tf')*[cx1;cy1;cz1];
            param4 = inv(Tf')*[cx2;cy2;cz2];
            Knl = tangential_stiffness_matrix(E,A,L,L0,I,diameter,t_gap,param1(1),param1(2),param1(3),param3(1),param3(2),param3(3),param2(1),param2(2),param2(3),param4(1),param4(2),param4(3));
            
            %质量矩阵
            eleM = mass_matrix(p,A,L,Ip);
            
            %波速
            Input.LocalDepth = (zz(noindex(2))+zz(noindex(1)))/2;
            Input.Time = i*t_gap;
            Mat = AiryWave(Input);
            %morison外载荷
            V_local = inv(Tf')*([Vx;Vy;0] + Mat(:,1));
            Va_local = inv(Tf')*Mat(:,2);
            node_speed_1 = [node_speed(noindex(1)*6-5);node_speed(noindex(1)*6-4);node_speed(noindex(1)*6-3)];
            node_speed_2 = [node_speed(noindex(2)*6-5);node_speed(noindex(2)*6-4);node_speed(noindex(2)*6-3)];
            node_accelerated_speed_1 = [node_accelerated_speed(noindex(1)*6-5);node_accelerated_speed(noindex(1)*6-4);node_accelerated_speed(noindex(1)*6-3)];
            node_accelerated_speed_2 = [node_accelerated_speed(noindex(2)*6-5);node_accelerated_speed(noindex(2)*6-4);node_accelerated_speed(noindex(2)*6-3)];
            elem_speed = 1/2 * inv(Tf')* (node_speed_1 + node_speed_2);
            elem_accelerated_speed = 1/2 * inv(Tf')*(node_accelerated_speed_1 + node_accelerated_speed_2);
            
            Fy=(pw*Cd*diameter*L/2)*(V_local(2) - elem_speed(2))*((V_local(2) - elem_speed(2))^2 + (V_local(3) - elem_speed(3))^2)^0.5 ...
                + pw*(1 + Ca)*pi*(diameter^2)/4*L*Va_local(2) - pw*Ca*pi*(diameter^2)/4*L*elem_accelerated_speed(2);
            Fz=(pw*Cd*diameter*L/2)*(V_local(3) - elem_speed(3))*((V_local(2) - elem_speed(2))^2 + (V_local(3) - elem_speed(3))^2)^0.5 ...
                + pw*(1 + Ca)*pi*(diameter^2)/4*L*Va_local(3) - pw*Ca*pi*(diameter^2)/4*L*elem_accelerated_speed(3);
            
            F_global = Tf'*[0;Fy;Fz];
            eleDof_temp = [noindex(1)*6-5 noindex(1)*6-4 noindex(1)*6-3];
            force(eleDof_temp) = force(eleDof_temp) + 1/2 *F_global;
            eleDof_temp = [noindex(2)*6-5 noindex(2)*6-4 noindex(2)*6-3];
            force(eleDof_temp) = force(eleDof_temp) + 1/2 *F_global;
            
            eleDof = [noindex(1)*6-5 noindex(1)*6-4 noindex(1)*6-3 noindex(1)*6-2 noindex(1)*6-1 noindex(1)*6 noindex(2)*6-5 noindex(2)*6-4 noindex(2)*6-3 noindex(2)*6-2 noindex(2)*6-1 noindex(2)*6];%3自由度刚度矩阵
            stiffness(eleDof,eleDof) = stiffness(eleDof,eleDof)+T'*eleK*T;%赋值并转为全局坐标
            nonlinear_stiffness(eleDof,eleDof) = nonlinear_stiffness(eleDof,eleDof)+T'*Knl*T;%赋值并转为全局坐标
            total_M(eleDof,eleDof) = total_M(eleDof,eleDof)+T'*eleM*T;
                      
            %局部求解内力  
            damp_matrix_ele = damp_param_2 * eleK;
            K_element = eleK + c0*eleM + c1*damp_matrix_ele;
            displacement_local = inv(T')*displacement_gap(eleDof);
            force_internal(eleDof) = inv(T')*force_internal(eleDof);  %
            force_internal(eleDof) = force_internal(eleDof) + K_element*displacement_local;
            force_internal(eleDof) = T'*force_internal(eleDof);
            
            force_shear_ele(j,:) =  eleK*(inv(T')*node_displacement_new(eleDof));
        end
       
        count_temp = count_temp + 1;
       
        
        damp_matrix = damp_param_2 * stiffness;
        K_effective = stiffness + c0*total_M + c1*damp_matrix;
        tangent_K = stiffness  + nonlinear_stiffness + c0*total_M + c1*damp_matrix;
        if (det(tangent_K)==0) 
            disp('矩阵奇异')
        end
        force_internal_1  = force_internal;
        force_internal_1(activeDof) = force_internal(activeDof)

         Q_effective(activeDof) = force(activeDof) + total_M(activeDof,activeDof)*(c0*node_displacement_new(activeDof)+c2*node_speed_new(activeDof) + c3*node_accelerated_speed_new(activeDof)) ...
        + damp_matrix(activeDof,activeDof)*(c1*node_displacement_new(activeDof)+c4*node_speed_new(activeDof) + c5*node_accelerated_speed_new(activeDof));
        
        displacement_gap(activeDof) = inv(K_effective(activeDof,activeDof))*(Q_effective(activeDof) - force_internal_1(activeDof));%P(a)此时的内力不清楚该怎么计算
        %force_internal(activeDof) = force_internal(activeDof) + tangent_K(activeDof,activeDof)*displacement_gap(activeDof);
        
        dpm_abs = abs(displacement_gap);
        if all(dpm_abs < 0.00005)
            label_1 = 0;
        end
        
        
        node_displacement = node_displacement_new;
        node_speed = node_speed_new;
        node_accelerated_speed = node_accelerated_speed_new;
        
        node_displacement_new(activeDof) = node_displacement_new(activeDof) + displacement_gap(activeDof);
        nodeCoord_new(:,1) = nodeCoord(:,1) + node_displacement_new(1:6:size(node_displacement_new(:,1)),1);
        nodeCoord_new(:,2) = nodeCoord(:,2) + node_displacement_new(2:6:size(node_displacement_new(:,1)),1);
        nodeCoord_new(:,3) = nodeCoord(:,3) + node_displacement_new(3:6:size(node_displacement_new(:,1)),1);
        
        %node_displacement(activeDof) = K_effective_1(activeDof,activeDof)\Q_effective(activeDof);

        node_accelerated_speed_new(activeDof) = c0*(node_displacement_new(activeDof) - node_displacement(activeDof)) - c2*node_speed(activeDof) - c3*node_accelerated_speed(activeDof);
        node_speed_new(activeDof) = node_speed(activeDof) + c6*node_accelerated_speed(activeDof) + c7*node_accelerated_speed_new(activeDof);
        temdisp(:,i)=[i;count_temp;node_displacement];
        temdisp1(:,i)=[i;count_temp;node_speed];
        temdisp2(:,i)=[i;count_temp;node_accelerated_speed];
    end
        
    node_accelerated_speed_new(activeDof) = c0*(node_displacement_new(activeDof) - node_displacement(activeDof)) - c2*node_speed(activeDof) - c3*node_accelerated_speed(activeDof);
    node_speed_new(activeDof) = node_speed(activeDof) + c6*node_accelerated_speed(activeDof) + c7*node_accelerated_speed_new(activeDof);
    node_displacement = node_displacement_new;
    node_speed = node_speed_new;
    node_accelerated_speed = node_accelerated_speed_new;
    temdisp(:,i)=[i;count_temp;node_displacement];
    temdisp1(:,i)=[i;count_temp;node_speed];
    temdisp2(:,i)=[i;count_temp;node_accelerated_speed];
end
displacement = node_displacement;
temdisp

%Draw deformation graph
maxlen=-1;
for i=1:numEle
    noindex=EleNode(i, :);
    deltax=xx(noindex(2))-xx(noindex(1));
    deltay=yy(noindex(2))-yy(noindex(1));
    L=sqrt(deltax * deltax+deltay * deltay);
    if(L>maxlen)
        maxlen=L;
    end
end
%Maximum displacement
maxabsDisp=0;
for i=1:numNode
    tempdispU=displacement(i*3-2);
    tempdispV=displacement(i*3-1);
    tempdisp=sqrt(tempdispU*tempdispU+tempdispV * tempdispV);
    if (tempdisp>maxabsDisp)
        maxabsDisp=tempdisp;
    end;
end
factor=0.1;
if(maxabsDisp > 1e-30)
    factor=scalefactor*maxlen/maxabsDisp;
end
%Graph element before deformation
figure
for i=1:numEle
    noindex=EleNode(i,:);
    Linel = plot3(xx([noindex]),yy([noindex]),zz([noindex]),':','Color',[0.4 0.4 0.4],'LineWidth',2);
    hold on
end
%Graph element after deformation
xx = nodeCoord_new(:,1);
yy = nodeCoord_new(:,2);
zz = nodeCoord_new(:,3);
xxDeformed=xx;
yyDeformed=yy;
zzDeformed=zz;
%Draw nodes
for i=1:numNode
    xxDeformed(i)=xxDeformed(i);
    yyDeformed(i)=yyDeformed(i);
    zzDeformed(i)=zzDeformed(i);
end
%Draw elements
for i=1: numEle
    noindex=EleNode(i, :);
    Line2=plot3(xxDeformed([noindex]),yyDeformed([noindex]),zzDeformed([noindex]),'-o','Color',[(abs(zzDeformed(noindex(1))))/max(abs(zzDeformed(:))) 0.4 0.4], 'LineWidth',2);
    hold on
end
for i=1:numEle
    noindex=EleNode(i,:);
    middlex=0.5 *(xxDeformed(noindex(2)) +xxDeformed(noindex(1)));
    middley=0.5 *(yyDeformed(noindex(2)) +yyDeformed(noindex(1)));
    middlez=0.5 *(zzDeformed(noindex(2)) +zzDeformed(noindex(1)));
    text( 'Color', [0 0.4 0],'FontSize', 12);
    hold on
end
for i=1:numNode
    tempstr=[' ' int2str(i)];%保留空格,用 stract 会没有空格
    text(xxDeformed(i),yyDeformed(i),zzDeformed(i),tempstr,'Color',[1 0 0],'FontSize', 12);
end
%Set up the coordinate axis

xlabel('X(m)');
ylabel('Y(m)');
zlabel('Z(m)');
legend([Linel, Line2], '未变形前', '变形后')